//////////////////////////////////////////Computor/////////////////////////////////////////////////////////////
//预测功能未添加，同时部分函数待完善
///////////////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef _COMPUTOR_H_
#define _COMPUTOR_H_

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include "../data_pack/DataPack.h"
#include "../detector/Detector.h"
#include "../../assert/Timer/Timer.h"
#include "../armor/Armor.h"
#include "../../serial/serial.h"
#include "../predictor/Kalman.h"
#include <vector>
#include <math.h>
#include <fstream>

//#define IMU
#define LIFECYCLE 3                     //装甲板队列内生存期在3秒以内的作为统计目标

using namespace cv;
using namespace Eigen;

typedef struct ArmorPack                //带有生命周期的装甲板
{
    Armor armor;
    Timer clock;

} ArmorPack;

class Computor
{
    using Matrix_R=Matrix3f;
    using Matrix_T=Vector3f;
public:
    float yaw=0;                        //发给电控的yaw轴电机转角（正左转，负右转）
    float pitch=0;                      //发给电控的pitch轴电机转角（正下转，负上转）
    float _yaw=0;                       //跟随模式下yaw;
    float _pitch=0;
    float _yaw_cam=0;
    float _pitch_cam=0;
    DataPack status;                                                            //云台状态数据包
    vector<ArmorPack> armor_queue;
    Armor target;
    Kalman<4,2> predictor;
private:
/////////////////////////////////////////姿态解算////////////////////////////////////////////////////////////////////////////
    Mat cam_to_imu_R2=(Mat_<float>(3,3)<<cos(-PI/2),      0,  -sin(-PI/2),         //相机坐标系到IMU系旋转矩阵R，固定常量   
                                                0,      1,      0,
                                        sin(-PI/2),      0,  cos(-PI/2));
    Mat cam_to_imu_R1=(Mat_<float>(3,3)<<cos(PI/2),      -sin(PI/2),  0,         //相机坐标系到IMU系旋转矩阵R，固定常量   
                                        sin(PI/2),   cos(PI/2),      0,
                                        0,      0,  1);                                    
    //Hero                 
    Mat cam_to_imu_T=(Mat_<float>(3,1)<<    0.1109,      0,       0.0695);         //相机坐标系到IMU系平移矩阵T，固定常量，从机械组获得
    Mat gun_to_imu_T=(Mat_<float>(3,1)<<    0.1924,      0,      0);             //枪口相对于云台原点的平移矩阵T，固定常量，从机械组获得
        
    //bubing
    //Mat cam_to_imu_T=(Mat_<float>(3,1)<<    11.09,      0,       6.95);         //相机坐标系到IMU系平移矩阵T，固定常量，从机械组获得
    //Mat gun_to_imu_T=(Mat_<float>(3,1)<<    19.24,      0,      0);             //枪口相对于云台原点的平移矩阵T，固定常量，从机械组获得
    
    Mat imu_to_inertial_R=(Mat_<float>(3,3)<<   1,      0,      0,              //IMU坐标系到云台坐标系（惯性系）旋转矩阵
                                                0,      1,      0,
                                                0,      0,      1);
    
    Mat inertial_to_imu_R=(Mat_<float>(3,3)<<   1,      0,      0,              //云台坐标系（惯性系）到IMU坐标系旋转矩阵
                                                0,      1,      0,
                                                0,      0,      1);
    
    Mat angular_velocity_imu=(Mat_<float>(3,1)<<    0,      0,      0);             //当前IMU角速度转化到相对于惯性系的角速度yaw-pitch-roll

    Mat angular_velocity_inertial=(Mat_<float>(3,1)<<    0,      0,      0);        //当前IMU角速度转化到相对于惯性系的角速度yaw-pitch-roll 

    Mat target_position_inertial=(Mat_<float>(3,1)<<    0,      0,      0);                //目标在惯性云台坐标系下位置X-Y-Z

    //Mat cam_to_yaw_T=(Mat_<float>(3,1)<<        0,      0,          0);         //相机到yaw轴平移矩阵

    //Mat cam_to_pitch_T=(Mat_<float>(3,1)<<      0,      0,          0);         //相机到pitch轴平移矩阵
    
    //Mat gun_to_pitch_T=(Mat_<float>(3,1)<<      0,      0,          0);         //枪口到pitch轴平移矩阵

    Matrix_R _cam_to_imu_R1;
    Matrix_R _cam_to_imu_R2;
    Matrix_T _cam_to_imu_T;
    Matrix_T _gun_to_imu_T;
    Matrix_R _imu_to_inertial_R;
    Matrix_R _inertial_to_imu_R;
    Matrix_T _target_position_inertial;
//////////////////////////////////////////////////重力补偿//////////////////////////////////////////////////

    float k1;                       //弹道系数k/m
    float g;                        //重力加速度
    float v;                        //初速度

    bool isRead_armor=false;
    bool isRead_imu=false;

    float newton_init;              //牛顿初始迭代点
//////////////////////////////////////////////////调参//////////////////////////////////////////////////////
    ofstream angle_pitch;           //弹道计算数据速输出流
    ofstream horizonal;
    ofstream vertical;

    bool isOpen=false;              //文件流是否打开
    bool isSaveEnd=false;           //是否保存结束
/////////////////////////////////////////////////卡尔曼滤波///////////////////////////////////////////////////
    
    int var_q1=200;
    int var_q2=200;
    int var_r1=200;
    int var_r2=200;
    int p0=100;
    Vector4f x_k=Vector4f::Zero();              //yaw pitch,Wy,Wp     
    Vector4f x_k1=Vector4f::Zero();
    Vector4f z_k=Vector4f::Zero();             //yaw,pitch
    Matrix4f A=Matrix4f::Identity();
    Matrix4f H=Matrix4f::Zero();
    Matrix4f Q=Matrix4f::Zero(); 
    Matrix4f R=Matrix4f::Zero();
    Matrix4f P=Matrix4f::Zero();

public:
    Computor();
    void updateArmor(const Detector& detector0);                                //更新新识别到的装甲板，并erase过期装甲板
    void updateStatus(DataPack& data);                                           //更新云台状态
    void getTarget();                                                           //计算抬枪角度
    void sendData(Serialport &serial0);                                         //发送数据
    void outPutStatus();                                                        //输出状态       
    void outPutTargetPosition();                                                //输出目标的惯性云台系的位置
    void computeTrajectory();                                                 //计算弹道系数时收集数据用
    void predict();
    ~Computor();
private:
    //根据弹道参数计算抬枪角度,init为初始迭代点,须传弧度
    float NewtonIteration(float init,float k1,float g,float v,float vertical_distence,float horizonal_distence);           
    float originFunction(float theata_before,float num1,float num2,float mum3);
    float deriveFunction(float theata_before,float num1,float num2);
    void kalman_init();
};

#endif